#ifndef _ALLGRASPS_H_
#define _ALLGRASPS_H_

#include "visgrabTypes.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <algorithm>
#include <iostream>

using boost::property_tree::ptree;

//! Class containing all grasps for all scenes. The class furthermore includes functions to write the experiment XML file.
/*!
 * This class contains the vector allSceneGrasps that contains all grasps 
 * per scene. When an object of this class is delete, all elements in the 
 * vector allSceneGrasps that it contains are deleted.
 *
 * Functions are included to write all grasps and information for all scenes as an experiment XML file
 */
template <class GraspT>
class AllGrasps {
public:
  AllGrasps(std::string _preShapesFile) {
    overallGraspID = 0;
    preShapesFile = _preShapesFile;
  };
  //! Destructor function, deletes all SceneGrasps in the allSceneGrasps vector.
  ~AllGrasps() { 
    for(unsigned int i=0; i<allSceneGrasps.size(); i++)
      delete allSceneGrasps[i];
  };
  //! Function to add scene grasps to the vector allSceneGrasps.
  /*!
   * Adds all grasps for the scene and the scene info to the vector allSceneGrasps
   * @param grasps Reference to the vector with all grasps for this scene
   * @param sceneInfo Pointer to the scene info.
   */
  void addSceneGrasps(std::vector<GraspT*> &grasps, SceneInfo* sceneInfo)  {
    // Give the overall grasp ID to the grasps
    for(unsigned int i=0; i<grasps.size(); i++)
      grasps[i]->overallGraspID = overallGraspID++;
    SceneGrasps<GraspT>* sceneGrasps = new SceneGrasps<GraspT>;
    sceneGrasps->grasps = grasps;
    sceneGrasps->sceneInfo = sceneInfo;    
    allSceneGrasps.push_back(sceneGrasps);
  };
  //! Function to print all scene grasps and info.
  void print()  {
    for(unsigned int i=0; i<allSceneGrasps.size(); i++) 
      allSceneGrasps[i]->print();
  };
  void addExperimentXMLGripper1(ptree &pt);
  //! Function to add gripper information to the experiment field. Three different versions for the three different grasp representations
  void addExperimentXMLGripper2(ptree &pt);
  void addExperimentXMLSceneInfo(ptree &pt, SceneInfo* sceneInfo);
  //! Function to add grasp information to the experiment field. Three different versions for the three different grasp representations
  void addExperimentXMLGrasp(ptree &pt, GraspT* grasp);
  //! Function to write the the experiment XML file.
  void writeExperimentXML(std::string xmlFile) {
    using boost::property_tree::ptree;
    ptree pt;
    

    for(unsigned int expI=0; expI<allSceneGrasps.size(); expI++) {
      ptree ptExp;
      ptExp.put<int>("<xmlattr>.id",expI);
      addExperimentXMLSceneInfo(ptExp,  allSceneGrasps[expI]->sceneInfo);
      addExperimentXMLGripper1(ptExp); 
      addExperimentXMLGripper2(ptExp); 
      for(unsigned int graspI=0; graspI<allSceneGrasps[expI]->grasps.size(); graspI++) {
	addExperimentXMLGrasp(ptExp, allSceneGrasps[expI]->grasps[graspI]);
      }
      pt.add_child("visgrab.experiment", ptExp); 
    }
    
    boost::property_tree::xml_writer_settings<char> settings('\t', 1);
    write_xml(xmlFile, pt, std::locale(), settings);

  };

  std::vector<SceneGrasps<GraspT>*> allSceneGrasps;  //!< Vector containing pointers to the SceneGrasps
  int overallGraspID;                           //!< Overall grasp ID is increased every time a grasp is added
  std::string preShapesFile;
};

template <>
AllGrasps<GraspRepresentationII>::AllGrasps(std::string _preShapesFile) {
  if(_preShapesFile=="") {
    std::cout << "ERROR: To use grasp representation type II, you need to specify the pre-shape file" << std::endl;
    std::cout << "Usage: prepareExperiments <VisgrabManifest.xml> <VisgrabExperiment.xml> <PreShapes.xml>" << std::endl;
    exit(EXIT_FAILURE);
  }
  overallGraspID = 0;
  preShapesFile = _preShapesFile;
};

//! Function to add the scene info to the property tree (thereby to the XML file).
/*!
 * @param pt Reference to a property tree, which corresponds to an experiment field in the XML file
 * @param sceneInfo Pointer to scene information
 */
template <class GraspT>
void AllGrasps<GraspT>::addExperimentXMLSceneInfo(ptree &pt, SceneInfo* sceneInfo) {
  //pt.put("sceneInfo.sceneLocator", sceneInfo->sceneLocator);
  //pt.put("sceneInfo.textureCondition", sceneInfo->textureCondition);
  //pt.put<int>("sceneInfo.nrObjects", sceneInfo->nrObjects);
  //for(unsigned int i=0; i<sceneInfo->objects.size(); i++)
  //  pt.add("sceneInfo.objects.object", sceneInfo->objects[i]);
  //pt.put("sceneInfo.sceneSet", sceneInfo->sceneSet);
  //pt.put("sceneInfo.sceneSetID", sceneInfo->sceneSetID);
  //pt.put<int>("sceneInfo.poseID", sceneInfo->poseID);
  //pt.put("sceneInfo.wc", sceneInfo->wc);
  //pt.put("sceneInfo.dwc", sceneInfo->dwc);
  pt.put("sceneInfo.sceneID", sceneInfo->sceneID);
  pt.put("sceneInfo.collision", sceneInfo->collision);
}

//! Function to add  gripper information to the experiment field in the property tree
template <class GraspT>
void AllGrasps<GraspT>::addExperimentXMLGripper1(ptree &pt) {
  using boost::property_tree::ptree;
  // Write the gripper info
  pt.put("gripper.name", "SchunkHand");
  pt.put("gripper.tcpframe", "SchunkHand.TCP");
};

template <>
void AllGrasps<GraspRepresentationI>::addExperimentXMLGripper2(ptree &pt) {
  /*! \brief
   * Function to add gripper information specific to GraspRepresentationI to the 
   * experiment field in the property tree
   */
  pt.add("gripper.graspcontroller", "GraspController");
};

template <>
void AllGrasps<GraspRepresentationII>::addExperimentXMLGripper2(ptree &pt) {
  /*! \brief 
   *Function to add gripper information specific to GraspRepresentationII to the experiment 
   * field in the property tree, including pre-shape information
   */
  using boost::property_tree::ptree;
  pt.add("gripper.graspcontroller", "GraspController");
  // Read the pre-shape info
  ptree ptPSin;
  read_xml(preShapesFile, ptPSin, boost::property_tree::xml_parser::trim_whitespace);
  BOOST_FOREACH(ptree::value_type &v, ptPSin.get_child("preshapes")) {
    if((std::string)v.first.data()=="preshape") {
      pt.add_child("gripper.preshape", v.second);
    }
  }
};

template <>
void AllGrasps<GraspRepresentationIII>::addExperimentXMLGripper2(ptree &pt) {
  /*! \brief
   * Function to add gripper information specific to GraspRepresentationIII to the experiment 
   * field in the property tree
   */
  pt.add("gripper.graspcontroller", "GraspController");
};

template <>
void AllGrasps<GraspRepresentationI>::addExperimentXMLGrasp(ptree &pt, GraspRepresentationI* grasp) {
  /*! \brief
   * Function to add contact-grasp information to the experiment field in the property tree.
   */
  ptree ptChild;
  ptChild.put<int>("<xmlattr>.graspID", grasp->overallGraspID);
  for(unsigned int i=0; i<grasp->contactPoints.size(); i++) {
    ptree ptCont;
    char posString[256];
    sprintf(posString, "%f %f %f", grasp->contactPoints[i]->x, grasp->contactPoints[i]->y, grasp->contactPoints[i]->z);
    ptCont.put("pos", posString);
    char dirString[256]; 
    sprintf(dirString, "%f %f %f", grasp->contactPoints[i]->d1, grasp->contactPoints[i]->d2, grasp->contactPoints[i]->d3);
    ptCont.put("dir", dirString);
    ptChild.add_child("contact", ptCont);
  }
  char approachString[256];
  sprintf(approachString, "%f %f %f", grasp->approachDirection[0], grasp->approachDirection[1], grasp->approachDirection[2]);
  ptChild.add("approachVector", approachString);
  char aOpenString[256];
  sprintf(aOpenString, "%f %f %f %f %f %f %f", grasp->aOpen[0], grasp->aOpen[1], grasp->aOpen[2], grasp->aOpen[3],
	  grasp->aOpen[4], grasp->aOpen[5], grasp->aOpen[6]);
  ptChild.add("aopen", aOpenString);
  char aCloseString[256];
  sprintf(aCloseString, "%f %f %f %f %f %f %f", grasp->aClose[0], grasp->aClose[1], grasp->aClose[2], grasp->aClose[3],
	  grasp->aClose[4], grasp->aClose[5], grasp->aClose[6]);
  ptChild.add("aclose", aCloseString);
  pt.add_child("contactgrasp", ptChild);
};

template <>
void AllGrasps<GraspRepresentationII>::addExperimentXMLGrasp(ptree &pt, GraspRepresentationII* grasp) {
  /*! 
   * \brief
   * Function to add pre-shape grasp information to the experiment field in the property tree.
   */
  ptree ptChild;
  ptChild.put<int>("<xmlattr>.graspID", grasp->overallGraspID);
  char posString[256], quatString[256];
  sprintf(posString, "%f %f %f", grasp->handPose->x, grasp->handPose->y, grasp->handPose->z);
  sprintf(quatString,"%f %f %f %f", grasp->handPose->q0, grasp->handPose->q1, grasp->handPose->q2, grasp->handPose->q3);
  ptChild.put("pose.position", posString);
  ptChild.put("pose.quaternion", quatString);
  ptChild.put("preshape.<xmlattr>.id", grasp->preShapeID);
  pt.add_child("preshapegrasp", ptChild);
};

template <>
void AllGrasps<GraspRepresentationIII>::addExperimentXMLGrasp(ptree &pt, GraspRepresentationIII* grasp) {
  /*! \brief Function to add joint-configuration grasp information to the experiment field in the property tree.
   */
  ptree ptChild;
  ptChild.put<int>("<xmlattr>.graspID", grasp->overallGraspID);
  char posString[256], quatString[256];
  sprintf(posString, "%f %f %f", grasp->handPose->x, grasp->handPose->y, grasp->handPose->z);
  sprintf(quatString,"%f %f %f %f", grasp->handPose->q0, grasp->handPose->q1, grasp->handPose->q2, grasp->handPose->q3);
  ptChild.put("pose.position", posString);
  ptChild.put("pose.quaternion", quatString);
  char qopenString[1024];
    sprintf(qopenString, "%f", grasp->jointConfOpen[0]);
  for(int i=1; i<NR_JOINTS_SDH; i++)
    sprintf(qopenString, "%s %f", qopenString, grasp->jointConfOpen[i]);
  ptChild.put("qopen", qopenString);
  char qcloseString[1024];
    sprintf(qcloseString, "%f", grasp->jointConfClose[0]);
  for(int i=1; i<NR_JOINTS_SDH; i++)
    sprintf(qcloseString, "%s %f", qcloseString, grasp->jointConfClose[i]);
  ptChild.put("qclose", qcloseString);
  char maxtauString[1024];
    sprintf(maxtauString, "%f", grasp->maxTau[0]);
  for(int i=1; i<NR_JOINTS_SDH; i++)
    sprintf(maxtauString, "%s %f", maxtauString, grasp->maxTau[i]);
  ptChild.put("maxtau", maxtauString);
  pt.add_child("grasp", ptChild);
};

#endif
